# coding:utf-8
from numpy.core.numeric import zeros, reshape, arange, array
from numpy.ma.core import sum, sin, cos, sqrt, arctan2
from numpy.linalg.linalg import solve
from numpy.lib.function_base import append
import math

class Gha(object):
    """一般化調和解析
    """
    
    def __init__(self, xn, fs):
        self.fs = fs
        self.fmax = fs / 2
        self.af = zeros(self.fmax)
        self.bf = zeros(self.fmax)
        self.M = zeros((2, 2))
        self.xn = reshape(xn, xn.size, 'F')[:xn.size / 2]
        self.nArray = arange(self.xn.size)
        self.L = self.xn.size / fs
        
    def calc(self, jmax):
        """メインルーチン
        """
        for j in xrange(jmax):
            for f in xrange(self.fmax):
                print j + 1, u'回目', f + 1, 'Hz'
                c = sum(self.xn * cos(2 * math.pi * (f + 1) * self.nArray / self.fs)) / self.fs
                s = sum(self.xn * sin(2 * math.pi * (f + 1) * self.nArray / self.fs)) / self.fs
                self.M[0, 0] = (sin(4 * math.pi * (f + 1) * self.L) + 4 * math.pi * (f + 1) * self.L) / (8 * math.pi * (f + 1))
                self.M[0, 1] = (1 - cos(4 * math.pi * (f + 1) * self.L)) / (8 * math.pi * (f + 1))
                self.M[1, 0] = self.M[0, 1]
                self.M[1, 1] = (-sin(4 * math.pi * (f + 1) * self.L) + 4 * math.pi * (f + 1) * self.L) / (8 * math.pi * (f + 1))
                AfBf = solve(self.M, array([c, s]))
                Af = AfBf[0]
                Bf = AfBf[1]
                #エネルギの算出
                Ef = sum(self.xn ** 2) / self.fs - 2 * (c * Af + s * Bf) + self.M[0, 0] * Af ** 2 + self.M[1, 1] * Bf ** 2 + self.M[0, 1] * Af * Bf
                if f == 0:
                    Emin = Ef
                    extf = f
                    exta = Af
                    extb = Bf
                if Ef < Emin:
                    Emin = Ef
                    extf = f
                    exta = Af
                    extb = Bf
            self.xn = self.xn - (exta * cos(2 * math.pi * (extf + 1) * self.nArray / self.fs) + extb * sin(2 * math.pi * (extf + 1) * self.nArray / self.fs))
            self.af[extf] += exta
            self.bf[extf] += extb
        freq = array([])
        amp = array([])
        phase = array([])
        m = 0
        for f in xrange(self.fmax):
            if self.af[f] ** 2 + self.bf[f] ** 2 > 0:
                freq = append(freq, f + 1)
                amp = append(amp, sqrt(self.af[f] ** 2 + self.bf[f] ** 2))
                phase = append(phase, arctan2(self.bf[f], self.af[f]))
                m += 1
        return freq, amp, phase